﻿using System;
using System.Collections.Generic;
using System.Text;
using RoverStudio.Common.Controller;
using RoverStudio.Common.PositionSource;
using RoverStudio.Math;
using RoverStudio.Common.WorldModel;

namespace CaterpillarRover
{
    public class TankHighLevelController : HighLevelController
    {


        private int stage;
        private Path path;
        private double errorRadius = 0.2;
        private volatile bool stopped = true;
        private volatile int stepCount = 0;
 
        private IPositionSource positionSource;

        public IPositionSource PositionSource
        {
            get { return positionSource; }
            set {
                if (positionSource != null)
                    positionSource.PositionUpdated -= new PositionUpdatedHandler(positionSource_PositionUpdated);
                positionSource = value;
                if (positionSource != null)
                    positionSource.PositionUpdated += new PositionUpdatedHandler(positionSource_PositionUpdated);
            }
        }



        private TankLineFollowerController lineFollowerController;

        public TankLineFollowerController LineFollowerController
        {
            get { return lineFollowerController; }
        }

        public TankHighLevelController(ITankController tankController, bool isWater)
        {
            //Initially not active
            this.isActive = false;

            this.lineFollowerController = new TankLineFollowerController(isWater);
            this.lineFollowerController.TankController = tankController;
            this.lineFollowerController.ControlSignalChanged += new ControlSignalChangedHandler(lineFollowerController_ControlSignalChanged);
        }

        private void lineFollowerController_ControlSignalChanged(object sender, ControlSignalEventArgs csea)
        {
            this.FireControlChangedEvent(csea.Alpha, csea.Distance, csea.VLeft, csea.VRight, csea.Time);
        }

        void positionSource_PositionUpdated(RoverStudio.Math.Vector3D position, RoverStudio.Math.Rotation orientation, DateTime time)
        {
            if (!this.isActive)
                return;

            if (stopped)
                return;
            if ((this.path == null) || (this.path.PathElements.Count == 1))
                return;
            //If fake position was detected
            if (Double.IsNaN(position.X) || Double.IsNaN(position.Y))
                return;

            if (stepCount >= 0)
            {
                if (stepCount == 0)
                {
                    stopped = true;
                    return;
                }
                stepCount--;
            }

            lock (this)
            {
                double distance = (this.path.PathElements[stage + 1] - position).Length2D();
                if (distance < errorRadius)
                {
                    stage++;
                    this.path.SelectedPathElementIndex = this.stage + 1;
                    if (stage >= this.path.PathElements.Count - 1)   //The rover has arrived.
                    {
                        lineFollowerController.Stop();
                        this.stopped = true;
                        this.stepCount = 0;
                        this.stage = 0;
                        this.path.SelectedPathElementIndex = this.stage + 1;
                        return;
                    }

                }
                lineFollowerController.SetLineToFollow(this.path.PathElements[stage], this.path.PathElements[stage + 1]);
                lineFollowerController.ContinueFollowing(position, orientation, time);
            }

        }

        public override void StartPathFollowing(RoverStudio.Common.WorldModel.Path path)
        {
            lock (this)
            {
                this.path = path;
            }
            //TODO: remove this.stage and use only path.SelectedPathElementIndex
            this.stage = 0;
            this.path.SelectedPathElementIndex = this.stage + 1;
            this.stopped = false;
            stepCount = 0;
         }

        public override void StopPathFollowing()
        {
            this.stopped = true;
            stepCount = 0;
            this.lineFollowerController.Stop();
            lock (this)
            {
                this.path = null;
            }
        }

        public override void PausePathFollowing()
        {
            this.stopped = true;
            stepCount = 0;
            this.lineFollowerController.Stop();
        }

        public override void ResumePathFollowing()
        {
            stepCount = -1;
            this.stopped = false;
        }

        public override void StepPathFollowing()
        {
            stepCount = 1;
            this.stopped = false;
        }
    }


    //class TankLineFollowerController
    //{
    //    public ITankController TankController
    //    {
    //        get { return null; }
    //        set { }
    //    }

    //    internal void Stop()
    //    {
    //        throw new NotImplementedException();
    //    }

    //    internal void SetLineToFollow(Vector3D vector3D, Vector3D vector3D_2)
    //    {
    //        throw new NotImplementedException();
    //    }

    //    internal void ContinueFollowing(Vector3D position, Rotation orientation, DateTime time)
    //    {
    //        throw new NotImplementedException();
    //    }
    //}
}
